#include "shoot.hpp"
#include "can.h"
#include "math.h"
#include "pid.h"
#include "tim.h"
#include "user_lib.h"
#include "tim.h"

fp32 friction_pid_speed_param[4][5] = {
8.0F, 0.1F, 0.0F, 10000, 700, 
8.0F, 0.1F, 0.0F, 10000, 700,
8.0F, 0.1F, 0.0F, 10000, 700, 
8.0F, 0.1F, 0.0F, 10000, 700};  // 摩擦轮 PID 参数
fp32 plunk_42_pid_speed_param[5]  = {10.0F, 1.0F, 0.0F, 20000, 10000};  // 拨弹轮 速度环PID 参数
fp32 plunk_42_pid_angle_param[5]  = {50.0F, 0.0F, 0.0F, 500, 500};      // 拨弹轮 角度环PID 参数
fp32 plunk42_speed                = 0.5;

/* 舵机角度解算函数 */
void Servo_Control_arm(uint16_t angle)
{
   float temp;
   temp =(1.0 / 9.0) * angle + 5.0;
 //  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (uint16_t )temp);
}

/**
 * @brief 构造函数
 *
 */
Shoot::Shoot(void) {
    is_stuck = 0;

    /* 摩擦轮 PID 初始化 */
    pid_init(&friction_pid_speed_[0], PID_POSITION, friction_pid_speed_param[0]);
    pid_init(&friction_pid_speed_[1], PID_POSITION, friction_pid_speed_param[1]);
	  pid_init(&friction_pid_speed_[2], PID_POSITION, friction_pid_speed_param[2]);
    pid_init(&friction_pid_speed_[3], PID_POSITION, friction_pid_speed_param[3]);
    /* 拨弹轮 PID 初始化 */
    pid_init(&plunk_pid_speed_, PID_POSITION, plunk_42_pid_speed_param);
		pid_init(&plunk_pid_angle_, PID_POSITION, plunk_42_pid_angle_param);


    /* 初始化摩擦轮斜坡函数 */
    ramp_init(&ramp_friction_[0]);
    ramp_friction_[0].out = 0;
		ramp_init(&ramp_friction_[1]);
    ramp_friction_[1].out = 0;
	
    behavior_.friction_speed_       = 0;
    behavior_.friction_state_       = FRICTION_STATE_STOP;
    behavior_.plunk_state_          = PLUNK_STATE_STOP;
    behavior_.pillwarehouse_state   = false;

    friction_state_ = FRICTION_STATE_STOP;
    plunk_state_    = PLUNK_STATE_STOP;


    friction_[0].expect_speed_ = 0;
    friction_[1].expect_speed_ = 0;
		friction_[2].expect_speed_ = 0;
    friction_[3].expect_speed_ = 0;
    plunk_.expect_speed_       = 0;
}

/**
 * @brief 发射机构初始化函数
 *
 */
void Shoot::init(void) {
    /* 初始化发射机构定时器 */
		plunk_expect_angle = plunk_real_angle;
	
    timer_ = rt_timer_create("shoot", timeout, (void *)this, 10, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(timer_);

    HAL_GPIO_WritePin(LASER_GPIO_Port, LASER_Pin, GPIO_PIN_SET);


    friction_state_ = FRICTION_STATE_STOP;
    plunk_state_    = PLUNK_STATE_STOP;
}
/**
 * @brief 发射机构控制定时器超时函数
 *
 * @param parameter
 */
void Shoot::timeout(void *parameter) {
    Shoot *t_shoot = (Shoot *)parameter;

    t_shoot->handle();
    // t_shoot->debug();
}
/**
 * @brief 发射机构周期控制函数
 *
 */
void Shoot::handle(void) {
    static uint16_t plunk_lock;

    plunk_lock = friction_action();
    plunk_action(plunk_lock);
    /* 摩擦轮速度 PID */
    friction_pid_set();
    /* 拨弹轮速度 PID */
    plunk_pid_set();

    /* 设置弹仓盖状态 */
//    contrl_pillwarehouse(behavior_.pillwarehouse_state);
    /* 设置摩擦轮和拨弹轮电流电压 */
    set_current(friction_[0].set_current_, friction_[1].set_current_, friction_[2].set_current_, friction_[3].set_current_);
}

/**
 * @brief 弹仓盖控制
 *
 */
void Shoot::contrl_pillwarehouse(bool state)
{
    if (state == false) {
        Servo_Control_arm(140);
    } else {
        Servo_Control_arm(45);
    }
}

/**
 * @brief 设置弹仓盖状态
 *
 */
void Shoot::set_pillwarehouse(bool state) {
    behavior_.pillwarehouse_state = state;
}

/**
 * @brief 读取弹仓盖状态
 *
 */
bool Shoot::get_pillwarehouse(void) {
    return behavior_.pillwarehouse_state;
}

/**
 * @brief 摩擦轮速度PID计算
 *
 */
void Shoot::friction_pid_set(void) {
	for(int i=0;i<4;i++)
    friction_[i].set_current_ = pid_calc(&friction_pid_speed_[i], friction_[i].speed_, friction_[i].expect_speed_);
}

/**
 * @brief 拨弹轮速度PID计算
 *
 */
void Shoot::plunk_pid_set(void) {
//    plunk_.set_voltage_ = pid_calc(&plunk_pid_speed_, plunk_.speed_, plunk_.expect_speed_);
	 fp32 error = plunk_expect_angle - plunk_real_angle;

    if (error > 50) {
        plunk_expect_angle = plunk_real_angle - 10;
        stuck_flag         = 1;
    }

    if (stuck_flag && abs(error) < 1) {
        stuck_flag = 0;
    }
    plunk_.expect_speed_ = -pid_calc(&plunk_pid_angle_, plunk_expect_angle, plunk_real_angle);
    plunk_.set_current_  = pid_calc(&plunk_pid_speed_, plunk_.speed_, plunk_.expect_speed_);
}


void Shoot::set_speed(uint16_t shoot42_speed_limit,uint16_t shoot42_speed_limit_2)
{
    FRICTION_42_SPEED = shoot42_speed_limit;
	  FRICTION_42_SPEED_2 = shoot42_speed_limit_2;
}
/**
 * @brief priction 状态设定消息队列
 *
 * @param friction_state_t 摩擦轮状态
 */
void Shoot::set_friction(friction_state_t friction_state, uint16_t friction_speed,uint16_t friction_speed_2) {
    behavior_.friction_state_ = friction_state;
    behavior_.friction_speed_ = friction_speed;
    behavior_.friction_speed_2 = friction_speed_2;
	
    if (friction_speed >= FRICTION_MAX_SPEED)
        behavior_.friction_speed_ = FRICTION_MAX_SPEED;
    else if (friction_speed <= FRICTION_MIN_SPEED)
        behavior_.friction_speed_ = FRICTION_MIN_SPEED;
		
		if (friction_speed_2 >= FRICTION_MAX_SPEED)
        behavior_.friction_speed_2 = FRICTION_MAX_SPEED;
    else if (friction_speed_2 <= FRICTION_MIN_SPEED)
        behavior_.friction_speed_2 = FRICTION_MIN_SPEED;
}

/**
 * @brief 设置拨弹轮状态
 *
 * @param plunk_state_ 拨弹轮状态
 *
 */
void Shoot::set_plunk(plunk_state_t plunk_state) {
    behavior_.plunk_state_ = plunk_state;
}
/**
 * @brief 推动弹丸进入弹链
 *
 */
void Shoot::shoot(void) {

			plunk_expect_angle += stuck_flag ? 0 : 0.8;  // 不卡弹的情况下，推动弹丸进入弹链

}
/**
 * @brief 拨弹轮退弹
 *
 */
void Shoot::tuidam(void) {
		
			plunk_expect_angle += stuck_flag ? 0: -0.8;  // 主动退弹
	
}
/**
 * @brief 读取摩擦轮状态
 *
 */
int Shoot::get_friction_state(void) {
    return behavior_.friction_state_;
}

/**
 * @brief 摩擦轮状态切换函数
 *
 * @return uint8_t 拨弹轮使能开关
 */
uint8_t Shoot::friction_action(void) {
    static uint16_t         plunk_lock;
    static friction_state_t last_state = FRICTION_STATE_STOP;

    plunk_lock = 1;

    if (behavior_.friction_state_ != last_state) {
        friction_state_ = behavior_.friction_state_;
        last_state      = behavior_.friction_state_;
    }
    /* 摩擦轮状态切换 */
    switch (friction_state_) {
        case FRICTION_STATE_SLOW_IN:
            /* 开启充能装置 */
            // HAL_GPIO_WritePin(POWERCTL_4_GPIO_Port, POWERCTL_4_Pin, GPIO_PIN_SET);
            /* 加速 */
            ramp_calc(&ramp_friction_[0], behavior_.friction_speed_, FRICTION_SPEEDUP_STEP);
            friction_[0].expect_speed_ = (-1) * ramp_friction_[0].out;
            friction_[1].expect_speed_ = ramp_friction_[0].out;
				    ramp_calc(&ramp_friction_[1], behavior_.friction_speed_2, FRICTION_SPEEDUP_STEP);
				    friction_[2].expect_speed_ = (-1) * ramp_friction_[1].out;
            friction_[3].expect_speed_ = ramp_friction_[1].out;
            /* 状态切换 */
            if (fabs((float)(behavior_.friction_speed_ + friction_[0].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP &&
                fabs((float)(behavior_.friction_speed_ - friction_[1].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP&&
								fabs((float)(behavior_.friction_speed_2 + friction_[2].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP &&
                fabs((float)(behavior_.friction_speed_2 - friction_[3].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP) {
                plunk_lock = 0;
            }
            break;
        case FRICTION_STATE_SLOW_OUT:
            /* 减速 */
            ramp_calc(&ramp_friction_[0], behavior_.friction_speed_, FRICTION_SPEEDUP_STEP + 40.0F);
            friction_[0].expect_speed_ = (-1) * ramp_friction_[0].out;
            friction_[1].expect_speed_ = ramp_friction_[0].out;
				    ramp_calc(&ramp_friction_[1], behavior_.friction_speed_2, FRICTION_SPEEDUP_STEP + 40.0F);
				    friction_[2].expect_speed_ = (-1) * ramp_friction_[1].out;
            friction_[3].expect_speed_ = ramp_friction_[1].out;
            /* 状态切换 */
            if (fabs((float)(behavior_.friction_speed_ + friction_[0].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP &&
                fabs((float)(behavior_.friction_speed_ - friction_[1].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP&&
								fabs((float)(behavior_.friction_speed_2 + friction_[2].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP &&
                fabs((float)(behavior_.friction_speed_2 - friction_[3].expect_speed_)) <= 1 * FRICTION_SPEEDUP_STEP) {
                plunk_lock = 0;
            }
            break;
        case FRICTION_STATE_READY:
            /* 使能拨弹轮 */
            plunk_lock = 0;
            break;
        case FRICTION_STATE_STOP:
            /* 关闭充能装置 */
            // HAL_GPIO_WritePin(POWERCTL_4_GPIO_Port, POWERCTL_4_Pin, GPIO_PIN_RESET);
            break;
    }
    return plunk_lock;
}

/**
 * @brief 拨弹轮状态切换函数
 *
 * @param plunk_lock 拨弹轮使能开关
 */
void Shoot::plunk_action(uint8_t plunk_lock) {
    static uint16_t plunk_flag = 0;

    static plunk_state_t last_state = PLUNK_STATE_STOP;

    if (behavior_.plunk_state_ != last_state) {
        plunk_state_ = behavior_.plunk_state_;
        last_state   = behavior_.plunk_state_;
    }

    /* 拨弹轮状态切换 */
    switch (plunk_state_) {
        case PLUNK_STATE_WORK:
            /* 卡弹处理 */
//            if (plunk_lock == 0) {
//                if (plunk_.torque_ < -9600 && plunk_.expect_speed_ < 0)
//                    plunk_flag = 60;
//                if (plunk_flag != 0) {
//                    plunk_flag--;
//                    is_stuck             = 1;
//                    plunk_.expect_speed_ = 2000;
//                } else {
//                    is_stuck             = 0;
//                    plunk_.expect_speed_ = -4500; /* 射频 */
//                }
//            } else {
//                plunk_.expect_speed_ = 0;
//            }
            break;
        case PLUNK_STATE_STOP:
            plunk_.expect_speed_ = 0;
            break;
    }
}
void Shoot::update(uint8_t id, uint8_t data[8]) {


		if (id == 1) {
        friction_[0].degree_     = (data[0] << 8) | data[1];
        friction_[0].speed_      = (data[2] << 8) | data[3];
        friction_[0].current_    = (data[4] << 8) | data[5];
        friction_[0].temprature_ = data[6];
    }
    if (id == 2) {
        friction_[1].degree_     = (data[0] << 8) | data[1];
        friction_[1].speed_      = (data[2] << 8) | data[3];
        friction_[1].current_    = (data[4] << 8) | data[5];
        friction_[1].temprature_ = data[6];
    }
		if (id == 3) {
        friction_[2].degree_     = (data[0] << 8) | data[1];
        friction_[2].speed_      = (data[2] << 8) | data[3];
        friction_[2].current_    = (data[4] << 8) | data[5];
        friction_[2].temprature_ = data[6];
    }
    if (id == 4) {
				friction_[3].degree_     = (data[0] << 8) | data[1];
        friction_[3].speed_      = (data[2] << 8) | data[3];
        friction_[3].current_    = (data[4] << 8) | data[5];
        friction_[3].temprature_ = data[6];
    }


    //rt_kprintf("%d, %d,%d, %d\r\n", (int16_t)friction_[0].speed_, (int16_t)friction_[1].speed_,(int16_t)friction_[2].speed_, (int16_t)friction_[3].speed_);
}

void Shoot::set_current(int16_t shoot_id_1, int16_t shoot_id_2,int16_t shoot_id_3,int16_t shoot_id_4)
{
    CAN_TxHeaderTypeDef tx_header;
    uint8_t             tx_data[8];
    uint32_t            tx_mailbox;

    /* 配置消息头 */
    tx_header.StdId              = 0x200U;
    tx_header.IDE                = CAN_ID_STD;
    tx_header.RTR                = CAN_RTR_DATA;
    tx_header.DLC                = 8;
    tx_header.TransmitGlobalTime = DISABLE;

    /* 计算发送数据 */
    tx_data[0] = shoot_id_1 >> 8;
    tx_data[1] = shoot_id_1;
    tx_data[2] = shoot_id_2 >> 8;
    tx_data[3] = shoot_id_2;
    tx_data[4] = shoot_id_3 >> 8;
    tx_data[5] = shoot_id_3;
    tx_data[6] = shoot_id_4 >> 8;
    tx_data[7] = shoot_id_4;

    HAL_CAN_AddTxMessage(&hcan2, &tx_header, tx_data, &tx_mailbox);
}
void Shoot::debug(void) {
    //rt_kprintf("%d, %d, %d, %d\n", friction_[0].set_current_, friction_[1].set_current_, friction_[2].set_current_, friction_[3].set_current_);
     rt_kprintf("%d,%d,%d,%d\n", friction_[0].speed_, friction_[1].speed_,friction_[2].speed_, friction_[3].speed_);
//    rt_kprintf("%d\n", plunk_.temprature_);
}